#include "uwb_dt/uwb_dt_handler.h"

#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "geometry_msgs/PoseArray.h"

#include <Eigen/Eigen>
#include <iostream>

template <typename T>
void TestTopicPub(uwb_dt_publisher& publisher, const uwb_dt_publisher::uwb_dt_param_t& param, const T& msg)
{
    publisher.publish(param, msg);
}

void TestString(uwb_dt_publisher& publisher)
{
    uwb_dt_publisher::uwb_dt_param_t param;
    param.topic_index = 0x01;
    param.id = 0x02;

    std_msgs::String test_string_msg;
    test_string_msg.data = "Hello World!";

    TestTopicPub(publisher, std::move(param), std::move(test_string_msg));
}

void TestGeometryMsgsPoseStamped(uwb_dt_publisher& publisher) 
{
    uwb_dt_publisher::uwb_dt_param_t param;
    param.topic_index = 0x02;
    param.id = 0x02;

    geometry_msgs::PoseStamped test_pose_msg;
    test_pose_msg.header.frame_id = "map";
    test_pose_msg.header.stamp = ros::Time::now();
    test_pose_msg.pose.position.x = 23.4343;
    test_pose_msg.pose.position.y = 32.4233443;
    test_pose_msg.pose.position.z = 8282.44343;
    test_pose_msg.pose.orientation.x = 2.323;
    test_pose_msg.pose.orientation.z = 991;

    TestTopicPub(publisher, std::move(param), std::move(test_pose_msg));
}


void TestBool(uwb_dt_publisher& publisher)
{
    uwb_dt_publisher::uwb_dt_param_t param;
    param.topic_index = 0x03;
    param.id = 0x02;

    std_msgs::Bool test_bool_msg;
    test_bool_msg.data = true;

    TestTopicPub(publisher, std::move(param), std::move(test_bool_msg));
}

void TestPoseArray(uwb_dt_publisher& publisher)
{
    uwb_dt_publisher::uwb_dt_param_t param;
    param.topic_index = 0x04;
    param.id = 0x02;

    geometry_msgs::PoseArray test_pose_array_msg;
    test_pose_array_msg.header.frame_id = "map";
    test_pose_array_msg.header.stamp = ros::Time::now();
    for (int i = 0; i < 3; i++)
    {
        geometry_msgs::Pose pose;
        pose.position.x = i;
        pose.position.y = i;
        pose.position.z = i;
        pose.orientation.x = i;
        pose.orientation.y = i;
        pose.orientation.z = i;
        pose.orientation.w = i;

        test_pose_array_msg.poses.push_back(pose);
    }

    TestTopicPub(publisher, std::move(param), std::move(test_pose_array_msg));
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "uwb_dt_test_node");
    ros::NodeHandle nh;

    uwb_dt_publisher publisher(nh);
    
    ros::Rate loop_rate(2);
    while(ros::ok())
    {   
        // TestString(publisher);
        // TestGeometryMsgsPoseStamped(publisher);
        // TestBool(publisher);
        TestPoseArray(publisher);

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}